{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "1542517c-4672-4ffc-9f41-6c7dad2c7fc2",
   "metadata": {},
   "source": [
    "# Demo：指星笔\n",
    "\n",
    "同济子豪兄 2024-5-13"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e7e93267-cae7-4971-9f1e-2e9bd9f00068",
   "metadata": {},
   "source": [
    "## 校准\n",
    "\n",
    "方向：让机械臂X轴正方向朝向正北\n",
    "\n",
    "水平：让桌面完全水平\n",
    "\n",
    "用手机内置的指南针和水准仪APP即可完成"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8892c3a2-3fa5-46fd-8cd1-5c6c1bdc87ba",
   "metadata": {},
   "source": [
    "## 导入工具包"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "f481bc76-fe42-4295-bddf-130ebecb81ea",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pymycobot.mycobot import MyCobot\n",
    "from pymycobot import PI_PORT, PI_BAUD\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6399e6b8-19b2-43db-b379-41a82e627c5e",
   "metadata": {},
   "source": [
    "## 连接机械臂"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "b107376e-cba1-4621-846e-a3fec46d5886",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc = MyCobot(PI_PORT, PI_BAUD)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d23f7a0d-ecee-469c-8df3-dd5d7727c0a9",
   "metadata": {},
   "source": [
    "## 机械臂归零"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "a4a0c68f-720e-4187-a1f7-7f8c714e38f4",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "df3fdf5a-000f-430f-8b51-f0e81d5ce48c",
   "metadata": {},
   "source": [
    "## 天文概念科普\n",
    "\n",
    "（Alt/Az）坐标系：地平坐标系，又称：高度/方位坐标系。\n",
    "\n",
    "Alt: 全称Altitude, 又称：高度、高度角、仰角、地平纬度、海拔标高。天体和观测者连线，与观测者所在地平面的夹角。\n",
    "\n",
    "当一个天体的alt是0°时，说明它在地平线上。高度增加，alt增加。高度减小，alt减小。\n",
    "\n",
    "Az：全称Azimuth, 又称：方位角、地平经度、右脚地平经度。天体由正北向东方的夹角。\n",
    "\n",
    "天球上所有天体都是自东向西运动的，\n",
    "\n",
    "当天体的az在0°~180°之间（北方-东方-南方，亦即子午线以东），高度处于上升状态。\n",
    "\n",
    "当天体的az方位在180°~360°之间（南方-西方-北方，亦即子午线以西），高度处于下降状态。"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "65885e4a-04a0-4b6d-a336-d24e783dcb42",
   "metadata": {},
   "source": [
    "## 获取天体的地平坐标（方位角、高度角）\n",
    "\n",
    "在手机上下载Stellerium虚拟天文台APP，打开陀螺仪，找到太阳、月球、火星、木星、土星、天狼星、北极星等天体的位置。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 79,
   "id": "2c1f3846-f335-4880-94bf-380c1e441d60",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 样例位置：北偏东30度，高度角60度\n",
    "Alt = 60\n",
    "Az = 30"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "95cac004-de1f-4496-b1d9-1a65c75dbf3e",
   "metadata": {},
   "source": [
    "## 计算机械臂电机角度\n",
    "\n",
    "方位角：由第1轴控制\n",
    "\n",
    "高度角：由第2轴控制"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "b5ed41a2-c435-430a-b8fc-5fc4ecc3c2b1",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-134\n"
     ]
    }
   ],
   "source": [
    "A1 = - Az\n",
    "A2 = Alt\n",
    "\n",
    "print(A1)\n",
    "\n",
    "if A1 < -170:\n",
    "    A1 += 360"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "c36629b1-fb03-47bc-ad15-eaa3a2f2c186",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-134"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "ed513e21-1490-46ae-accf-10e998212d0a",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_angles([A1, A2, 0, 0, 0, 0], 40)\n",
    "time.sleep(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "dca87d8e-3288-4179-a821-5adbfc7f28c0",
   "metadata": {},
   "outputs": [],
   "source": [
    "def point_star_angles(Alt=30, Az=0):\n",
    "    \n",
    "    A1 = - Az\n",
    "    A2 = Alt\n",
    "    \n",
    "    if A1 < -170:\n",
    "        A1 += 360\n",
    "\n",
    "    if (A1 > 170) and (A1 < 190):\n",
    "        print('超出第1轴转动范围，对正南方附近20度无效')\n",
    "        return\n",
    "\n",
    "    mc.send_angles([A1, A2, 0, 0, 0, 0], 40)\n",
    "    time.sleep(2)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0d5ac0fb-65f8-4597-9662-1eeef691b2fd",
   "metadata": {},
   "source": [
    "## 测试"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "7ea33371-437e-4589-81e2-e2937f65f0cc",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 机械臂归零\n",
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "43945a97-4e22-4546-b2c9-35228d1054f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置一：仰角30度，正北\n",
    "point_star_angles(30, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "id": "06ac4530-f4e2-466c-82c5-9773eab053a9",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置二：仰角60度，北偏东30度\n",
    "point_star_angles(60, 30)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 89,
   "id": "5de2baeb-7450-46dc-91a3-f3d98cb1c4e2",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置三：仰角45度，正东\n",
    "point_star_angles(45, 90)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 90,
   "id": "43c467e2-b121-4451-b3d7-7d86fe6c905f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置四：仰角50度，正东南\n",
    "point_star_angles(50, 135)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 91,
   "id": "a28e4c31-7ebf-4634-be9d-f15a6f5f77f7",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置五：仰角30度，正西南\n",
    "point_star_angles(30, 225)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 92,
   "id": "cf824dd6-362b-4762-bf9f-e7e22e10833f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置六：仰角45度，正西\n",
    "point_star_angles(45, 270)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 93,
   "id": "c945f661-b78e-461a-818f-79768e774305",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置七：仰角55度，正西北\n",
    "point_star_angles(55, 315)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 94,
   "id": "28f88a10-1093-4906-95d7-eadbb8a7b72d",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "超出第1轴转动范围，对正南方附近20度无效\n"
     ]
    }
   ],
   "source": [
    "# 位置八：仰角55度，正南\n",
    "point_star_angles(55, 180)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e813bf70-acde-42a8-97e8-645177d289b8",
   "metadata": {},
   "source": [
    "## 坐标控制\n",
    "\n",
    "平动自由度不变，Rx为0不变，只调节Ry、Rz即可\n",
    "\n",
    "Ry是高度角的余角\n",
    "\n",
    "Rz控制方位角，0为正北，正向偏西，负向偏东"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "f9a57c75-6484-49d6-938e-16a1ddae7e8e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 机械臂归零\n",
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "498dd0b2-14e7-4450-9e37-f4d441aecbd3",
   "metadata": {},
   "outputs": [],
   "source": [
    "start_position = [100, -50, 280]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "id": "c2e763c6-f522-41ba-9741-6f44960d6395",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 回到坐标控制初始位置\n",
    "mc.send_coords(start_position+[0, 0, 0], 10)\n",
    "time.sleep(5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "9499f8a2-e1a1-4d01-900b-ac372b515d10",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 北偏东30度，仰角30度\n",
    "mc.send_coords(start_position+[0, 60, -30], 10)\n",
    "time.sleep(5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "a05fc4cb-b93d-4370-b7a9-8e7b653e0d97",
   "metadata": {},
   "outputs": [],
   "source": [
    "start_position = [100, -50, 280]\n",
    "\n",
    "def point_star_coords(Alt=30, Az=60):\n",
    "    Ry = 90 - Alt\n",
    "    Rz = -Az\n",
    "    \n",
    "    mc.send_coords(start_position+[0, Ry, Rz], 10)\n",
    "    time.sleep(2)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ecc7d087-e198-42c3-9a6d-fe6a9103a82a",
   "metadata": {},
   "source": [
    "## 测试"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "cd4fb03d-44dd-407e-b898-95eeb3cc2265",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 机械臂归零\n",
    "mc.send_angles([0, 0, 0, 0, 0, 0], 40)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "b30c6063-2bf2-413c-947c-cf6b23d9c710",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 回到坐标控制初始位置\n",
    "mc.send_coords(start_position+[0, 0, 0], 10)\n",
    "time.sleep(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "965098ff-0de4-4a6e-9a8a-498cf9d01495",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置一：仰角30度，正北\n",
    "point_star_coords(30, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "id": "c0f78669-ce96-42bb-abff-ea4b9f326618",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置二：仰角60度，北偏东30度\n",
    "point_star_coords(60, 30)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "e60304fb-a478-4636-aa72-84fe7b78fb1b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置三：仰角45度，正东\n",
    "point_star_coords(45, 90)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "edf05064-f79d-42c4-8e6b-e4c572ae82eb",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置四：仰角50度，正东南\n",
    "point_star_coords(50, 135)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "ca497ede-52fd-413f-81fb-064851a2404a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置五：仰角55度，正南\n",
    "point_star_coords(55, 180)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "ccc37a7c-d295-4fb5-972b-067a527d009d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置六：仰角30度，正西南\n",
    "point_star_coords(30, 225)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "id": "30649656-53eb-479d-bf26-af1ef827b18b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置七：仰角45度，正西\n",
    "point_star_coords(45, 270)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "id": "7e494650-3351-41fb-9b94-1bf79744048f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 位置八：仰角55度，正西北\n",
    "point_star_coords(55, 315)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ea38a88e-b3dd-459b-a59b-147498853c80",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
